#ifndef MAT_HPP_
# define MAT_HPP_

# include <math.h>
# include "Vec.hpp"

# ifndef M_PI
#  define M_PI	3.1415926535
# endif // !M_PI

#ifdef _MSC_VER
// Disable float -> double warning.
# pragma warning(disable: 4305)
# pragma warning(disable: 4244)
// Disable exception warning
#pragma warning(disable: 4290)
#endif

template <unsigned int N, typename T>
class		Mat
{
public:
	Mat()
	{
	}

	inline void		display()
	{
		for (unsigned int c = 1; c <= _size; c++)
		{
			std::cout << "\t" << _mat[c - 1];
			if (c % N == 0)
				std::cout << std::endl;
		}
	}

	inline void		init(const T value = 0)
	{
		for (unsigned int c = 0; c < _size; c++)
		{
			_mat[c] = value;
		}
	}

	Mat&			operator=(Mat const& right)
	{
		for (unsigned int c = 0; c < _size; c++)
		{
			_mat[c] = right._mat[c];
		}
		return *this;
	}

	inline T		operator()(const unsigned int y, const unsigned int x) const
	{
		return _mat[y * N + x];
	}

	inline T&		operator()(const unsigned int y, const unsigned int x)
	{
		return _mat[y * N + x];
	}

	inline void		loadIdentity()
	{
		init();
		for (unsigned int c = 0; c < N; c++)
		{
			_mat[c * N + c] = 1;
		}
	}

	void			translation(Vec<N - 1, T> const& translation)
	{
		Mat<N, T>	translationMat;

		translationMat.loadIdentity();
		for (unsigned int c = 0; c < N - 1; c++)
		{
			translationMat._mat[c * N + (N - 1)] = translation[c];
		}
		*this = *this * translationMat;
	}

	void				scale(Vec<N - 1, T> const& scale)
	{
		Mat<N, T>		scaleMat;
		unsigned int	c;

		scaleMat.init();
		for (c = 0; c < N - 1; c++)
		{
			scaleMat._mat[c * N + c] = scale[c];
		}
		scaleMat._mat[c * N + c] = 1;
		*this = *this * scaleMat;
	}

	void			rotation(double angle, Vec<N - 1, T> axe)
	{
		if (N == 4)
		{
			Mat		rotation;
			
			rotation.init();
			axe.normalize();
			angle = angle * M_PI / 180.0;
			rotation._mat[0] = axe[0] * axe[0] * (1.0 - cosf(angle)) + cosf(angle);
			rotation._mat[1] = axe[0] * axe[1] * (1.0 - cosf(angle)) - axe[2] * sinf(angle);
			rotation._mat[2] = axe[0] * axe[2] * (1.0 - cosf(angle)) + axe[1] * sinf(angle);

			rotation._mat[4] = axe[0] * axe[1] * (1.0 - cosf(angle)) + axe[2] * sinf(angle);
			rotation._mat[5] = axe[1] * axe[1] * (1.0 - cosf(angle)) + cosf(angle);
			rotation._mat[6] = axe[1] * axe[2] * (1.0 - cosf(angle)) - axe[0] * sinf(angle);

			rotation._mat[8] = axe[0] * axe[2] * (1.0 - cosf(angle)) - axe[1] * sinf(angle);
			rotation._mat[9] = axe[1] * axe[2] * (1.0 - cosf(angle)) + axe[0] * sinf(angle);
			rotation._mat[10] = axe[2] * axe[2] * (1.0 - cosf(angle)) + cosf(angle);
			
			rotation._mat[15] = 1.0;

			*this = *this * rotation;
		}
	}

	Vec<N, T>		operator*(Vec<N, T> const& right)
	{
		static Vec<N, T>	result;
		int					y, x;

		result.init();
		for (y = 0; y < N; y++)
		{
			for (x = 0; x < N; x++)
			{
				result[y] += _mat[y * N + x] * right[x];
			}
		}
		return result;
	}

	Mat				operator*(Mat const& right)
	{
        static Mat      result;
        unsigned int	x, y, it;

		result.init();
		for (y = 0; y < N; y++)
		{
			for (x = 0; x < N; x++)
			{
				for (it = 0; it < N; it++)
					result._mat[N * x + y] += _mat[N * x + it] * right._mat[N * it + y];
			}
		}
		return result;
	}

	Mat&		operator*=(Mat const& right)
	{
		*this = *this * right;
		return *this;
	}

	T*			get() const
	{
		return (T*)_mat;
	}

protected:
    static const unsigned  int	_size = N * N;
	T					_mat[_size];
};

typedef Mat<3, float>	Mat3f;
typedef Mat<4, float>	Mat4f;

typedef Mat<3, int>		Mat3i;
typedef Mat<4, int>		Mat4i;

#endif // !MAT_HPP_
